/**
  ******************************************************************************
  * @file    servo.c
  * @brief   舵机驱动实现 舵机1采用PC6 舵机2采用PC7(TIM3)
  * @version V1.0.0
  * @date    2025-07-15
  * @note    
  ******************************************************************************
  */
	
#include "servo.h"

/**
  * @brief 舵机1初始化
  * @retval None
  */
void servo1_init()
{
	HAL_TIM_PWM_Init(&servo_TIM);
	HAL_TIM_PWM_Start(&servo_TIM,servo1_TIM_CHANNEL);
	__HAL_TIM_SET_COMPARE(&servo_TIM, servo1_TIM_CHANNEL, 0.5 / 20.0 * PWM_PERIOD_COUNT); //设置初始化位置
}

/**
  * @brief 舵机2初始化
  * @retval None
  */
void servo2_init()
{
	HAL_TIM_PWM_Init(&servo_TIM);
	HAL_TIM_PWM_Start(&servo_TIM,servo2_TIM_CHANNEL);
	__HAL_TIM_SET_COMPARE(&servo_TIM, servo2_TIM_CHANNEL, 0.5 / 20.0 * PWM_PERIOD_COUNT); //设置初始化位置
}

/**
  * @brief 设置定时器占空比
  * @param dutyfactor：占空比
	* @param servo：选择servo1_TIM_CHANNEL或servo2_TIM_CHANNEL	
  * @retval None
  */
void set_dutyfactor(float dutyfactor, uint32_t servo)
{
	/* 计算上下限值 */
	float lower_limit = 0.5 / 20.0 ;	//0.5表示0°
	float upper_limit = 2.5 / 20.0 ;	//2.5表示最大度数

	/* 边界处理 */
	if (dutyfactor < lower_limit) 
	{
		dutyfactor = lower_limit;
	} 
	else if (dutyfactor > upper_limit) 
	{
		dutyfactor = upper_limit;
	}
	if (servo == servo1_TIM_CHANNEL)
	{
		__HAL_TIM_SetCompare(&servo_TIM,servo1_TIM_CHANNEL,dutyfactor * PWM_PERIOD_COUNT);
	}
	else if (servo == servo2_TIM_CHANNEL)
	{
		__HAL_TIM_SetCompare(&servo_TIM,servo2_TIM_CHANNEL,dutyfactor * PWM_PERIOD_COUNT);
	}
}

/**
  * @brief 设置舵机角度
  * @param angle_temp：角度
	* @param servo：选择servo1_TIM_CHANNEL或servo2_TIM_CHANNEL	
  * @retval None
  */
void set_servo_angle(uint16_t angle_temp,uint32_t servo)
{
	float angle = (0.5 + angle_temp / 270.0 * (2.5 - 0.5)) / 20.0 ;    // 计算角度对应的占空比
	set_dutyfactor(angle, servo);    // 设置占空比
}
